#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h> 
#include <sensor_msgs/Joy.h>

using namespace std;
 
class LogTeleop
{
public:
    LogTeleop();
 
private:
    /* data */
    void LogCallback(const sensor_msgs::Joy::ConstPtr& Joy);
    ros::NodeHandle n;
    ros::Subscriber sub ;
    ros::Publisher pub ;
    double vel_linear,vel_angular;
    int axis_angular,axis_linear,ton;
};
 
LogTeleop::LogTeleop()
{
    // 手柄摇杆的转动幅度
    n.param<int>("axis_linear",axis_linear,1);
    n.param<int>("axis_angular",axis_angular,2);
    // 线速度和角速度
    n.param<double>("vel_linear",vel_linear,0.1);
    n.param<double>("vel_angular",vel_angular,0.4);
    n.param<int>("button",ton,6);
    // 发布与订阅
    pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1, true);
    sub = n.subscribe<sensor_msgs::Joy>("joy",10,&LogTeleop::LogCallback,this);
}
 
void LogTeleop::LogCallback(const sensor_msgs::Joy::ConstPtr& Joy)
{
	geometry_msgs::Twist twist;
	std_msgs::String string;

	vel_linear = vel_linear * (1 + (Joy->axes[5] * 0.1));
	if(vel_linear>=0.2)
	   vel_linear = 0.2;
	else if (vel_linear<=0)
	   vel_linear = 0;
	else
	   vel_linear = vel_linear;
	
	vel_angular = vel_angular * (1 +  (Joy->axes[4] * 0.1));
	if(vel_angular>=0.6)
		vel_angular = 0.6;
	else if(vel_angular<=0)
		vel_angular = 0;
	else
		vel_angular = vel_angular;

	// 控制时，需要同时按住6号键
	if(Joy->buttons[ton])
	{
	    twist.linear.x =(Joy->axes[axis_linear])*vel_linear;
	    twist.linear.y = 0;
	    twist.linear.z = 0;
	    
	    twist.angular.x = 0;
	    twist.angular.y = 0;
	    twist.angular.z =(Joy->axes[axis_angular])*vel_angular;
		    
		ROS_INFO("linear:%.3lf angular:%.3lf",twist.linear.x,twist.angular.z);
		pub.publish(twist);
	}
}

int main(int argc,char** argv)
{
    ros::init(argc, argv, "logitech_teleop_key");
    LogTeleop  logteleop;
    ros::spin();
    return 0;
}
